public class Motion{

   Track track;
	double t;			// the total time, which should never be more than 1 (time-unit)
	double vXNew;		// the speed of the car in X direction
	double vYNew;		// the speed of the car in Y direction
	int xNew;			// the x coordinate of the car
	int yNew;			// the y coordinate of the car
	int mirrorX;		// Indicator to mirror all movements in positive direction
	int mirrorY;		// Idem for Y
	double rewardZum =0 ;
	boolean debug=false; // switches print commands on console
	boolean[][] racefogclean = new boolean[100][100];
	
	

   public Motion(Track thisTrack){
		track=thisTrack;
		t=0.0;
		vXNew = 0.0;
		vYNew = 0.0;
		xNew= 0;
		yNew= 0;
		mirrorX=1;
		mirrorY=1;
		
   }
   public void resetshadow(){
	   for (int i=0 ; i<100 ; i++){
		   for (int v=0 ; v<100 ; v++){
			   racefogclean[i][v]=false;
		   }
	   }
   }

  
   
   public Car move(Car thisCar){

		vXNew = thisCar.getVX();
		vYNew = thisCar.getVY();
		xNew= thisCar.getX();
		yNew= thisCar.getY();
		t=0.0;
		mirrorX=1;
		mirrorY=1;

		int stepX=1;		// The size of the move in the X direction to compensate 1 move in the Y direction
		int stepY=1;		// Vice versa
		int restX=0;		// If vX and vY are not proportional, this is what has to be moved at the end.
		int restY=0;		// Idem

		boolean ok=true;	// flag to control the number of times a loop is passed
		int loopCount=0;	// flag to control the number of times a loop is passed

		//REMARK: break is acceleration in a negative direction

		// Mirror the speed values to a positive direction
		if(vXNew<0){
			mirrorX=-1;
			vXNew*=-1;
		}
		if(vYNew<0){
			mirrorY=-1;
			vYNew*=-1;
		}

if(debug){ System.out.println("MirrorX/Y:\t" + mirrorX + "  ,  " + mirrorY);}

		// The agent will move as close to the direction as possible, without making diagonal moves.
		// So if the speed is (2,4) the movement will be (1,2) and (1,2).
		// (18,5) will be (3,1)(3,1)(3,1)(3,1)(3,1)(3,0)
		// (11,0) will be (11,0)

		if(vXNew>vYNew){
			// If (relative) speed in X direction equal or bigger than in Y direction, do more steps in X for each step in Y

if(debug){ System.out.println("Pos:"+ xNew+" | "+yNew+"\t"+"\t    vXNew  >  vYNew");}
if(debug){ System.out.print("total speed X-Y\t ");}
if(debug){ System.out.format("%.4f", vXNew*mirrorX);}
if(debug){ System.out.print("\t");}
if(debug){ System.out.format("%.4f%n", vYNew*mirrorY);}

			if(vYNew<0.5){
				ok=false;
				restX=(int) Math.rint(vXNew);
			}else{
				restX =(int)(Math.rint(vXNew) % Math.rint(vYNew));
				stepX=(int)((Math.rint(vXNew)-restX)/Math.rint(vYNew));
			}
			stepY=(int) Math.rint(vYNew);

if(debug){ System.out.println("Pos:"+ xNew+" | "+yNew+"\t"+"StepX=" + stepX + "\tStepY=" + stepY + "\trestX=" + restX);}

			// this loop will continue until either: t>1 or decel=0 (crash!!) or doLoops have been carried out
			while(ok && (loopCount++<stepY)){

if(debug){ System.out.println("OK: move in X direction "+stepX+" steps "+stepY+" times");}
	         ok=moveStep(true, stepX, thisCar);

if(debug && ok){ System.out.println("OK: move in Y direction 1 step "+stepY+" times");}
				if(ok) {ok=moveStep(false, 1, thisCar);}

if(debug){ System.out.print("Pos:"+ xNew+" | "+yNew+"\t"+"total time         ");}
if(debug){ System.out.format("%.4f%n", t);}
         } // end while( ok && (loopCount...

if(debug){ System.out.println("Rest: move in X direction "+restX+" steps");}
			moveStep(true, restX, thisCar);

if(debug){ System.out.print("Pos:"+ xNew+" | "+yNew+"\t"+"total time         ");}
if(debug){ System.out.format("%.4f%n", t);}

		}else{

			// If speed in Y direction bigger than in X direction, do more steps in Y for each step in X

if(debug){ System.out.println("Pos:"+ xNew+" | "+yNew+"\t"+"\t    vYNew  >  vXNew");}
if(debug){ System.out.print("total speed X-Y\t ");}
if(debug){ System.out.format("%.4f", vXNew*mirrorX);}
if(debug){ System.out.print("\t");}
if(debug){ System.out.format("%.4f%n", vYNew*mirrorY);}

			if(vXNew<0.5){
				ok=false;
				restY=(int) Math.rint(vYNew);
			}else{
				restY =(int)(Math.rint(vYNew) % Math.rint(vXNew));
				stepY=(int)((Math.rint(vYNew)-restY)/Math.rint(vXNew));
			}
			stepX=(int) Math.rint(vXNew);

if(debug){ System.out.println("Pos:"+ xNew+" | "+yNew+"\t"+"StepX=" + stepX + "\tStepY=" + stepY + "\trest Y=" + restY);}

			// this loop will continue until either: t>1 or decel=0 (crash!!) or doLoops have been carried out
			while(ok && (loopCount++<stepX)){

if(debug){ System.out.println("OK: move in Y direction "+stepY+" steps "+stepX+" times");}
	         ok=moveStep(false, stepY, thisCar);

if(debug && ok){ System.out.println("OK: move in X direction 1 step "+stepX+" times");}
				if(ok) {ok=moveStep(true, 1, thisCar);}
			} // end while (ok &&....

if(debug){ System.out.println("Rest: move in Y direction "+restY+" steps");}
			moveStep(false, restY, thisCar);

if(debug){ System.out.println("Pos:"+ xNew+" | "+yNew+"\t"+"total time         "+t);}

		}

if(debug){ System.out.print("End values vX: ");}
if(debug){ System.out.format("%.2f", vXNew*mirrorX);}
if(debug){ System.out.print(" vY: ");}
if(debug){ System.out.format("%.2f%n", vYNew*mirrorY);}
if(debug){ System.out.println("");}
if(debug){ System.out.println("");}


		// store the new values in the car
		thisCar.setVX(vXNew*mirrorX);
		thisCar.setVY(vYNew*mirrorY);

		thisCar.setX(xNew);
		thisCar.setY(yNew);

thisCar.setReward(rewardZum);
		rewardZum = 0;
		return thisCar;
	}


//---------------------------------------------------------------------------------------//

	// Take doStep steps in X-direction (if moveX=true) or Y-direction (if moveX=false)
	private boolean moveStep(boolean moveX, int doStep, Car thisCar){

   	int stopCode=0;
      int stepCount=0;

		// this loop will continue until either: t>1 or decel=0 (crash!!) or doStep steps have been taken
		while((stopCode==0) && (stepCount++<doStep)){
			// Check if the move is possible
			if(moveX){
				stopCode=checkStep(vXNew, t, xNew+mirrorX, yNew);			// t is a class variable
			}else{
				stopCode=checkStep(vYNew, t, xNew, yNew+mirrorY);			// t is a class variable
			}

			// Step has been checked. Now what?
			if (stopCode==1){					// time has run out
				 // do no more steps
				 return false;
			}else if (stopCode==2){			// car crashed
				vXNew=0;
				vYNew=0;
				// do no more steps
				return false;
			}else {															// No problems so move to new square
				//System.out.println(racefogclean[33][33]);
				if(racefogclean[xNew][yNew]==false){
					racefogclean[xNew][yNew]= true;
					rewardZum = rewardZum + 0.3;
				}
				
				if(moveX){
					t+=1/vXNew;												// calculate new time
					xNew+=mirrorX;											// do a step in X direction
					vXNew-=track.getDecel(xNew,yNew)/vXNew;			// get Deceleration and calc new speed
				}else{
					t+=1/vYNew;												// calculate new time
					yNew+=mirrorY;											// do a step in Y direction
					vYNew-=track.getDecel(xNew,yNew)/vYNew;			// get Deceleration and calc new speed
				}
if(debug){ System.out.print("Updates: xNew: "+ xNew+" yNew: "+yNew+" vXNew: ");}
if(debug){ System.out.format("%.4f", vXNew*mirrorX);}
if(debug){ System.out.print(" vYNew: ");}
if(debug){ System.out.format("%.4f", vYNew*mirrorY);}
if(debug){ System.out.print(" t: ");}
if(debug){ System.out.format("%.4f%n", t);}

		      if(track.checkPointPass(xNew, yNew, thisCar.getCheckpoint())){   // update checkpoint
					thisCar.incCheckpoint();
if(debug){ System.out.print("CHECKPOINT on "+ xNew+"-"+yNew);}
				}
			} // end if (stopCode==1)-else-else
		} // end while

if(debug){ System.out.print("OK:"+ doStep+" steps moved ");}
if(debug){ if(moveX) {System.out.println("in x-direction ");}else{System.out.println("in y-direction ");}}
		return true;
	} // end method moveStep

//---------------------------------------------------------------------------------------//

	private int checkStep(double v, double tNew, int x, int y){

		double decel=0.0;  // Deceleration


		tNew+=1/v;										// calculate time after 1 step
     	if(tNew>1){
     		
if(debug){ System.out.println("Time out: "+tNew+" v= "+v);}

			return 1;
		}else{
			decel=track.getDecel(x,y);			// get Deceleration
			if(decel>0.99){			// CRASH!!
				//flag crash aanzetten

if(debug){ System.out.println("Pos:"+ x+" | "+y+"\t"+"CRASH");}

				return 2;
			} // end if(decel...)
		} // end if(t>1)-else
		return 0;
	} // end method checkStep



}
